
#include <iostream>
#include <thread>
#include "stereo_capture.h"
#include "imu_capture.h"
#include "gps_nmea.h"
#include <math.h>



using namespace std;


#define DEG_TO_RAD      (M_PI / 180.0f)
#define RAD_TO_DEG      (180.0f / M_PI)



// scaling factor from 1e-7 degrees to meters at equater
// == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
#define LOCATION_SCALING_FACTOR 0.011131884502145034f
// inverse of LOCATION_SCALING_FACTOR
#define LOCATION_SCALING_FACTOR_INV 89.83204953368922f


#define SBUF_SIZE 					512


FILE *g_imuDataFp;
FILE *g_imageTimeStampFp;
FILE *g_imageTimeStampFpAndName;
FILE *g_gpsDataFp;
FILE *g_gpsPosFp;
GPS_Status g_gpsStatus;
bool g_firstGps3DFix;

struct Location g_origin;


bool writeImuDataToFile(const char * format, ...);
bool writemageTimeStampToFile(const char * format, ...);
bool writeImageTimeStampFpAndNameToFile(const char * format, ...);
bool writeGpsDataToFile(const char * format, ...);
bool writeGpsPosToFile(const char * format, ...);


void videoHandler();
static void leftVideoCallbackHandler(cv::Mat image, long long timestamp);
static void rightVideoCallbackHandler(cv::Mat image, long long timestamp);

void imuHandler();
static void imuDataCallbackHandler(float accX, float accY, float accZ,
										float gyroX, float gyroY, float gyroZ, long long timestamp);


static long long getNanosecondsTimestamp()
{
    auto now = std::chrono::system_clock::now();
    auto duration = now.time_since_epoch();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count();
}


template <class T>
T constrain_value(const T amt, const T low, const T high)
{
    // the check for NaN as a float prevents propagation of floating point
    // errors through any function that uses constrain_value(). The normal
    // float semantics already handle -Inf and +Inf
    if (isnan(amt)) {
        return (low + high) / 2;
    }

    if (amt < low) {
        return low;
    }

    if (amt > high) {
        return high;
    }

    return amt;
}


/*
 * Constrain a value to be within the range: low and high
 */
template <class T>
T constrain_value(const T amt, const T low, const T high);

inline float constrain_float(const float amt, const float low, const float high)
{
    return constrain_value(amt, low, high);
}


float longitude_scale(const struct Location &loc)
{
#if HAL_CPU_CLASS < HAL_CPU_CLASS_150
    static int32_t last_lat;
    static float scale = 1.0;
    // don't optimise on faster CPUs. It causes some minor errors on Replay
    if (labs(last_lat - loc.lat) < 100000) {
        // we are within 0.01 degrees (about 1km) of the
        // same latitude. We can avoid the cos() and return
        // the same scale factor.
        return scale;
    }
    scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
    scale = constrain_float(scale, 0.01f, 1.0f);
    last_lat = loc.lat;
    return scale;
#else
    float scale = cosf(loc.lat * 1.0e-7f * DEG_TO_RAD);
    return constrain_float(scale, 0.01f, 1.0f);
#endif
}


/*
  return the distance in meters in North/East/Down plane as a N/E/D vector
  from loc1 to loc2
 */
void location_3d_diff_NED(const struct Location &loc1, const struct Location &loc2, Vector3f &pos)
{
	pos.x = (loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR;
	pos.y = (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1);
	pos.z = (loc1.alt - loc2.alt) * 0.01f;
}


int main()
{
	g_firstGps3DFix = false;
	//start imu data to file
	g_imuDataFp =  fopen("./Data/imudata.txt", "ab+");
	if(g_imuDataFp == NULL)
	{
		cout << "open imudata.txt error" << endl;
		return -1;
	}

	//image timestamp to file
	g_imageTimeStampFp = fopen("./Data/imageTimeStamp.txt", "ab+");
	if(g_imageTimeStampFp == NULL)
	{
		cout << "open imageTimeStamp.txt error" << endl;
		return -1;
	}

	//image timestamp and image name to file
	g_imageTimeStampFpAndName = fopen("./Data/imageTimeStampAndName.txt", "ab+");
	if(g_imageTimeStampFpAndName == NULL)
	{
		cout << "open imageTimeStampAndName.txt error" << endl;
		return -1;
	}

	g_gpsDataFp = fopen("./Data/gpsData.txt", "ab+");
	if(g_gpsDataFp == NULL)
	{
		cout << "open gpsData.txt error" << endl;
		return -1;
	}

	g_gpsPosFp = fopen("./Data/gpsPos.txt", "ab+");
	if(g_gpsPosFp == NULL)
	{
		cout << "open gpsPos.txt error" << endl;
		return -1;
	}


	//write imu data header
	writeImuDataToFile("#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\n");

	//write image data header
	writeImageTimeStampFpAndNameToFile("#timestamp [ns],filename\n");

	//write gps data header
	writeGpsDataToFile("#timestamp [ns],long, lat, alt, vx, vy, vz\n");

	cout << "imu data capture" << endl;
	std::thread imuCaptureThread(imuHandler);
	ImuCaptureHandler.registerimuDataCallback(imuDataCallbackHandler);

	//cout << "data capture" << endl;
	//start stereo capture thread
	std::thread stereoCaptureThread(videoHandler);

	StereoCaptureHandler.registerStereoLeftVideoCallback(leftVideoCallbackHandler);
	StereoCaptureHandler.registerStereoRightVideoCallback(rightVideoCallbackHandler);

	cout << "gps init...." << endl;
	GpsNmeaHandler.gps_init();

	while(1)
	{
		if(g_imuDataFp != NULL)
		{
			fflush(g_imuDataFp);
		}

		if(g_imageTimeStampFp != NULL)
		{
			fflush(g_imageTimeStampFp);
		}

		if(g_imageTimeStampFpAndName != NULL)
		{
			fflush(g_imageTimeStampFpAndName);
		}

		if(g_gpsDataFp != NULL)
		{
			fflush(g_gpsDataFp);
		}

		if(g_gpsPosFp != NULL)
		{
			fflush(g_gpsPosFp);
		}

		// 100ms update, gps update 5hz
		usleep(100*1000);

		GpsNmeaHandler.gps_read();

		g_gpsStatus = GpsNmeaHandler.state.status;

		if(g_gpsStatus > GPS_OK_FIX_3D)
		{
			if(!g_firstGps3DFix)
			{
				g_firstGps3DFix = true;
				g_origin = GpsNmeaHandler.state.location;
			}

			long long timestamp = getNanosecondsTimestamp();

			//save gps data
			writeGpsDataToFile("%lld,%d,%d,%d,%f,%f,%f\n", timestamp,
											GpsNmeaHandler.state.location.lng,
											GpsNmeaHandler.state.location.lat,
											GpsNmeaHandler.state.location.alt,
											GpsNmeaHandler.state.velocity.x,
											GpsNmeaHandler.state.velocity.y,
											GpsNmeaHandler.state.velocity.z);
			Vector3f pos;
			location_3d_diff_NED(g_origin, GpsNmeaHandler.state.location, pos);
			writeGpsPosToFile("%lld, %f,%f,%f,%f,%f,%f,%f\n", timestamp, pos.x, pos.y, pos.z, 0, 0, 0, 1);
		}
	}

	imuCaptureThread.join();
	stereoCaptureThread.join();

	if(g_imuDataFp != NULL)
	{
		fclose(g_imuDataFp);
		g_imuDataFp = NULL;
	}

	if(g_imageTimeStampFp != NULL)
	{
		fclose(g_imageTimeStampFp);
		g_imageTimeStampFp = NULL;
	}

	if(g_imageTimeStampFpAndName != NULL)
	{
		fclose(g_imageTimeStampFpAndName);
		g_imageTimeStampFpAndName = NULL;
	}
	return 0;
}


void videoHandler()
{
	bool ret = StereoCaptureHandler.startVideoCapture();

	if(!ret)
	{
		cout << " start video error." << endl;
		return;
	}
}



static void leftVideoCallbackHandler(cv::Mat image, long long timestamp)
{
	if(!g_firstGps3DFix)
	{
		return;
	}

	cv::Mat im;
	im = image.clone();
	//cout << "left timestamp:" << timestamp << endl;
	char nameStr[50];
	memset(nameStr, 0, sizeof(nameStr));
	sprintf(nameStr, "./Data/left/%lld.png", timestamp);
	writemageTimeStampToFile("%lld\n", timestamp);
	writeImageTimeStampFpAndNameToFile("%lld,%lld\n", timestamp, timestamp);
	cv::imwrite(nameStr, image);
}


static void rightVideoCallbackHandler(cv::Mat image, long long timestamp)
{
	if(!g_firstGps3DFix)
	{
		return;
	}

	cv::Mat im;
	im = image.clone();
	//cout << "right timestamp:" << timestamp << endl;
	char nameStr[50];
	memset(nameStr, 0, sizeof(nameStr));
	sprintf(nameStr, "./Data/right/%lld.png", timestamp);
	cv::imwrite(nameStr, image);
}



void imuHandler()
{
	bool ret = ImuCaptureHandler.imuInit();

	if(!ret)
	{
		cout << "imu handler error." << endl;
		return;
	}
}


static void imuDataCallbackHandler(float accX, float accY, float accZ,
										float gyroX, float gyroY, float gyroZ, long long timestamp)
{
	if(!g_firstGps3DFix)
	{
		return;
	}

	//cout << timestamp << "," << accX << "," << accY << "," << accZ << "," << gyroX << "," << gyroY << "," << gyroZ << endl;
	writeImuDataToFile("%lld,%f,%f,%f,%f,%f,%f\n",timestamp, accX, accY, accZ, gyroX, gyroY, gyroZ);
}




bool writeImuDataToFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == g_imuDataFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, g_imuDataFp);
    if (size != n) {
        fclose(g_imuDataFp);
		g_imuDataFp = NULL;
		cout << "writeImuDataToFile fialed" << endl;
        return false;
    }

	return true;
}


bool writemageTimeStampToFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == g_imageTimeStampFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, g_imageTimeStampFp);
    if (size != n) {
        fclose(g_imageTimeStampFp);
		g_imageTimeStampFp = NULL;
		cout << "writemageTimeStampToFile fialed" << endl;
        return false;
    }

	return true;
}

bool writeImageTimeStampFpAndNameToFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == g_imageTimeStampFpAndName)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, g_imageTimeStampFpAndName);
    if (size != n) {
        fclose(g_imageTimeStampFpAndName);
		g_imageTimeStampFpAndName = NULL;
		cout << "g_imageTimeStampFpAndName fialed" << endl;
        return false;
    }

	return true;
}


bool writeGpsDataToFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == g_gpsDataFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, g_gpsDataFp);
    if (size != n) {
        fclose(g_gpsDataFp);
		g_gpsDataFp = NULL;
		cout << "g_gpsDataFp fialed" << endl;
        return false;
    }

	return true;
}

bool writeGpsPosToFile(const char * format, ...)
{
	size_t size, n;
	char sbuf[SBUF_SIZE];

	if(NULL == g_gpsPosFp)
	{
		return false;
	}

	va_list args;
	va_start (args, format);
	n = vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	size = fwrite(sbuf, 1, n, g_gpsPosFp);
    if (size != n) {
        fclose(g_gpsPosFp);
		g_gpsPosFp = NULL;
		cout << "g_gpsPosFp fialed" << endl;
        return false;
    }

	return true;
}
